package legway;

import lejos.nxt.*;
import lejos.robotics.subsumption.*;
import lejos.util.TextMenu;

/**
 * This is the Legways main program. It starts a buttonListener, that makes sure it is possible
 * to exit the program at any time. It initializes the balance controller and the behaviors associated
 * with the Legway.
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 */
class Legway
{
	private static final String ON = " on ";
	private static final String OFF = " off";
	private static final String[] ITEM_NAMES = new String[] {"Start","Avoid Front","Follow Line","Calib. Light","Calib. Gyro"};
	
	private static String[] menuItems = ITEM_NAMES.clone();
	private static CtrlParam ctrl;
	private static boolean[] behaviorsEnabled = new boolean[5];
	
	
	public static void main(String[] args) {
		Button.ESCAPE.addButtonListener(new ButtonListener() {
			public void buttonPressed(Button b) {
				Sound.buzz();
				System.exit(0);
			}
			public void buttonReleased(Button b) { }
		});

		ctrl = new CtrlParam();
		BalanceController bc = new BalanceController(ctrl);

		// Instantiating behaviors
		Behavior b1 = new FollowLine(ctrl);
		Behavior b2 = new BTDriver(ctrl);
		Behavior b3 = new AvoidFront(ctrl);
		Behavior[] bArray = { b1, b2, b3 };
		Arbitrator arby = new Arbitrator(bArray);

		// Menu
		prependEnabled();
		TextMenu menu = new TextMenu(menuItems, 0,"Legway Menu");
		int selected = 0;
		try {
			while((selected = menu.select(selected)) != 0) {
				behaviorsEnabled[selected] = !behaviorsEnabled[selected];
				prependEnabled();
				
				if(selected == 1) // AvoidFront
					AvoidFront.setEnabled(behaviorsEnabled[selected]);
				else if(selected == 2) // Follow Line
					FollowLine.setEnabled(behaviorsEnabled[selected]);
				else if(selected == 3) // Calib. Light
					FollowLine.calcThreshold();
				else if(selected == 4) // Calib. Gyro
					GyroscopeSensor.calcOffset();
			}
		}
		catch (Exception e) {
			System.exit(0);
		}

		LCD.clear();
		bc.start();
		arby.start();
	}
	
	private static void prependEnabled() {
		// Mark selected
		for(int i = 1; i < 3; i++) {
			if(behaviorsEnabled[i])
				menuItems[i] = ITEM_NAMES[i] + ON;
			else
				menuItems[i] = ITEM_NAMES[i] + OFF;
		}
	}
}
